#ifdef __cplusplus
extern "C" {
#endif
	
#include "InertiaMeasure.h"
#include "devi2c.h"
#include "delay.h"
#include "math.h"
#define IMU_CAL_CNT 10
#define IMU_Kp                          (10.0f )
#define IMU_Ki                          (0.008f)
#define IMU_DEAL_TIME                   (10)    /* sampling period unit ms */
/* half of the sampling period */
#define halfT (IMU_DEAL_TIME / 1000.0 / 2)
/* the initial position and attitude angles are: 0, 0, 0, corresponding to quaternions: 1, 0, 0, 0 */
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
/* integral of the error */
float exInt = 0, eyInt = 0, ezInt = 0;

static uint8_t ImuType = 0;
/**
  * @brief   写数据到MPU6050寄存器
  * @param   reg_add:寄存器地址
	* @param	 reg_data:要写入的数据
  * @retval  
  */
uint8_t Imu_WriteData(uint8_t DevAddr,uint8_t RegAddr,uint8_t data){
	DevDmaI2c_WriteReg(&data,DevAddr<<1,RegAddr,1);
//	DevHwI2C_WriteByte(DevAddr,RegAddr,data);
	return 0;
}
uint8_t Imu_ReadByte(uint8_t DevAddr, uint8_t ReadAddr){
	uint8_t desBuf=0;
	DevDmaI2c_ReadReg(&desBuf,DevAddr<<1,ReadAddr,1);
	return desBuf;
}
uint8_t Imu_ReadBuf(uint8_t* ReadBuf, uint8_t DevAddr, uint8_t ReadAddr, uint8_t NumByte){
	DevDmaI2c_ReadReg(ReadBuf,DevAddr<<1,ReadAddr,NumByte);
	return 0;
}
/**
  * @brief  设置MPUxxxx陀螺仪传感器满量程范围
  * @param  fs: selects the full scale range.
  *         - 0: ±250 dps
  *         - 1: ±500 dps
  *         - 2: ±1000 dps
  *         - 3: ±2000 dps
  * @retval i2c status.
  */
uint8_t Imu_SetGyroFsr(uint8_t srcVal){
	return Imu_WriteData(IMU_ADDR,IMU_GYRO_CFG_REG,(srcVal<<3)|3);//设置陀螺仪满量程范围  
}
/**
  * @brief  设置MPUxxxx加速度传感器满量程范围
  * @param  fs: selects the full scale range.
  *         - 0: ±2g
  *         - 1: ±4g
  *         - 2: ±8g
  *         - 3: ±16g
  * @retval i2c status.
  */
uint8_t Imu_SetAccelFsr(uint8_t srcVal){
	return Imu_WriteData(IMU_ADDR,IMU_ACCEL_CFG_REG,srcVal<<3);//设置加速度传感器满量程范围  
}
/**
  * @brief  set the accelerometer and gyroscope low pass filter.
  * @param  lpf: 0~7.
  *         +---------------------------+-----------------+--------+
  *         |DLPF_CFG|   Accelerometer  |        Gyroscope         |
  *         |  lpf   |   (Fs = 1kHz)    |--------------------------|
  *         |        |Bandwidth  |Delay |Bandwidth | Delay|Fs (kHz)|
  *         |        |(Hz)       |(ms)  |(Hz)      | (ms) |        |
  *         |--------+-----------+------+----------+------+--------|
  *         |0       |260        |0     | 256      |0.98  | 8      |
  *         |1       |184        |2.0   | 188      |1.9   | 1      |
  *         |2       |94         |3.0   | 98       |2.8   | 1      |
  *         |3       |44         |4.9   | 42       |4.8   | 1      |
  *         |4       |21         |8.5   | 20       |8.3   | 1      |
  *         |5       |10         |13.8  | 10       |13.4  | 1      |
  *         |6       |5          |19.0  | 5        |18.6  | 1      |
  *         |--------+------------------+-----------------+--------+
  *         |7       |    RESERVED      |   RESERVED      | 8      |
  *         +---------------------------+-----------------+--------+
  * @retval i2c status.
  */
uint8_t Imu_SetLpf(uint16_t srcVal){
	uint8_t data=0;
	if(srcVal>=188)data=1;
	else if(srcVal>=98)data=2;
	else if(srcVal>=42)data=3;
	else if(srcVal>=20)data=4;
	else if(srcVal>=10)data=5;
	else data=6; 
	return Imu_WriteData(IMU_ADDR,IMU_CFG_REG,data);//设置数字低通滤波器  
}
/**
  * @brief  sample rate divider set.
  * @param  rate: sample rate(0~255).
  *         - sample rate = gyroscope output rate / (1 + SMPLRT_DIV)
  * @retval i2c status.
  */
uint8_t Imu_SetRate(uint16_t srcVal){
	uint8_t data;
	if(srcVal>1000)srcVal=1000;
	if(srcVal<4)srcVal=4;
	data=1000/srcVal-1;
	Imu_WriteData(IMU_ADDR,IMU_SAMPLE_RATE_REG,data);			//设置数字低通滤波器
 	return Imu_SetLpf(srcVal/2);								//自动设置LPF为采样率的一半
}
/**
  * @brief  Inertial measurement unit initialization.
  * @param  rate: sample rate(0~255).
  *         - sample rate = gyroscope output rate / (1 + SMPLRT_DIV)
  * @retval i2c status.
  */
uint8_t InertiaMeasure_Init(uint8_t srcType){
	uint8_t res=0;
	ImuType = srcType;
	Imu_WriteData(IMU_ADDR,IMU_PWR_MGMT1_REG,0X80);				//复位MPUxxxx
	delay_ms(100);
	Imu_WriteData(IMU_ADDR,IMU_PWR_MGMT1_REG,0X00);				//唤醒MPUxxxx
	Imu_SetGyroFsr(3);					        				//陀螺仪传感器,±2000dps
	Imu_SetAccelFsr(3);					       	 				//加速度传感器,±2g
	Imu_SetRate(50);						       	 			//设置采样率50Hz
	Imu_WriteData(IMU_ADDR,IMU_INT_EN_REG,0X00);   				//关闭所有中断
	Imu_WriteData(IMU_ADDR,IMU_USER_CTRL_REG,0X00);				//I2C主模式关闭
	Imu_WriteData(IMU_ADDR,IMU_FIFO_EN_REG,0X00);				//关闭FIFO
	Imu_WriteData(IMU_ADDR,IMU_INTBP_CFG_REG,0X82);				//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
	res=Imu_ReadByte(IMU_ADDR,IMU_DEVICE_ID_REG);  				//读取MPU6500的ID
	if(res==IMU_ID1||res==IMU_ID2){ 							//器件ID正确
		Imu_WriteData(IMU_ADDR,IMU_PWR_MGMT1_REG,0X01);  		//设置CLKSEL,PLL X轴为参考
		Imu_WriteData(IMU_ADDR,IMU_PWR_MGMT2_REG,0X00);  		//加速度与陀螺仪都工作
		Imu_SetRate(50);						       			//设置采样率为50Hz   
	}else return 1;
	if(1==ImuType){
		res=Imu_ReadByte(AK8963_ADDR,MAG_WIA);    					//读取AK8963 ID   
		if(res==AK8963_ID){
			Imu_WriteData(AK8963_ADDR,MAG_CNTL2,0X01);				//复位AK8963
			Imu_WriteData(AK8963_ADDR,MAG_CNTL1,0X11);				//设置AK8963为单次测量
		}else return 1;
	}
	return 0;
}
/**
  * @brief  Inertial measurement unit check.
  * @retval Inertial measurement unit status.
  */
uint8_t InertiaMeasure_Check(void){
	uint8_t res=0,outVal=0;
	res=Imu_ReadByte(IMU_ADDR,IMU_DEVICE_ID_REG);  				//读取MPU6500的ID
	if(res==IMU_ID1||res==IMU_ID2){ 							//器件ID正确
		outVal  = 1;
		outVal |= 1<<1;
	}
	if(1==ImuType){
		res=Imu_ReadByte(AK8963_ADDR,MAG_WIA);    					//读取AK8963 ID   
		if(res==AK8963_ID){
			outVal |= 1<<2;
		}
	}else{
		outVal |= 1<<2;
	}
	return	outVal;
}
/**
  * @brief  accelerometer / gyroscope / temperature data get.
  * @param  mpu: data struct.
  * @retval error_status.
  */
uint8_t Imu_GetData(ImuHandleType *mpu)
{
	uint8_t buf[14];
	int16_t data;

	Imu_ReadBuf(buf,IMU_ADDR,IMU_ACCEL_XOUTH_REG, 14);
	{
		mpu->ax = ((uint16_t)buf[0] << 8) + buf[1] - mpu->ax_cal;
		mpu->ay = ((uint16_t)buf[2] << 8) + buf[3] - mpu->ay_cal;
		mpu->az = ((uint16_t)buf[4] << 8) + buf[5] - mpu->az_cal;
		data = ((uint16_t)buf[6] << 8) | buf[7];
		mpu->temp = 36.53 + data / 340.0;
		mpu->gx = ((uint16_t)buf[8] << 8)  + buf[9]  - mpu->gx_cal;
		mpu->gy = ((uint16_t)buf[10] << 8) + buf[11] - mpu->gy_cal;
		mpu->gz = ((uint16_t)buf[12] << 8) + buf[13] - mpu->gz_cal;
		return SUCCESS;
	}
	return ERROR;
}
/**
  * @brief  calibrate offset.
  * @param  mpu: data struct.
  * @retval none.
  */
uint8_t Imu_Calibrate(ImuHandleType *mpu)
{
  uint16_t i;
  int32_t temp[6];
  for(i = 0; i < 6; i++){
    temp[i] = 0;
  }

  for(i = 0; i < 20; i++){
    delay_ms(20);
    Imu_GetData(mpu);
  }
  for(i = 0; i < IMU_CAL_CNT; i++){
    delay_ms(20);
    Imu_GetData(mpu);
    temp[0] += mpu->ax;
    temp[1] += mpu->ay;
    temp[2] += mpu->az;
    temp[3] += mpu->gx;
    temp[4] += mpu->gy;
    temp[5] += mpu->gz;
  }
  mpu->ax_cal = temp[0] / IMU_CAL_CNT;
  mpu->ay_cal = temp[1] / IMU_CAL_CNT;
  mpu->az_cal = temp[2] / IMU_CAL_CNT - 16384;
  mpu->gx_cal = temp[3] / IMU_CAL_CNT;
  mpu->gy_cal = temp[4] / IMU_CAL_CNT;
  mpu->gz_cal = temp[5] / IMU_CAL_CNT;
  return SUCCESS;
}

/**
  * @brief  mpu6050 data deal(complementary filtering mode).
  * @param  mpu:  mpu: data struct.
  * @retval none.
  */
uint8_t Imu_MpuDataDeal(ImuHandleType *mpu){
	static float angle_z = 0;
	int16_t x;
	float temp;

	float gx, gy, gz, ax, ay, az;
	float q0temp, q1temp, q2temp, q3temp;
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;

	float q0q0 = q0 * q0;
	float q0q1 = q0 * q1;
	float q0q2 = q0 * q2;
	float q1q1 = q1 * q1;
	float q1q3 = q1 * q3;
	float q2q2 = q2 * q2;
	float q2q3 = q2 * q3;
	float q3q3 = q3 * q3;

	/* unit g */
	ax = mpu->ax * 9.8 / 16384;
	ay = mpu->ay * 9.8 / 16384;
	az = mpu->az * 9.8 / 16384;

	/* unit radians/s */
	gx = mpu->gx * 250 / 57.3 / 32768;
	gy = mpu->gy * 250 / 57.3 / 32768;
	gz = mpu->gz * 250 / 57.3 / 32768;

	/* when the additive is in a free falling state, attitude calculation is not performed */
	if(ax * ay * az==0){
		return 1;
	}

	norm = sqrt(ax * ax + ay * ay + az * az);
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;

	vx = 2 * (q1q3 - q0q2);
	vy = 2 * (q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;

	ex = (ay * vz - az * vy);
	ey = (az * vx - ax * vz);
	ez = (ax * vy - ay * vx);

	exInt = exInt + ex * IMU_Ki;
	eyInt = eyInt + ey * IMU_Ki;
	ezInt = ezInt + ez * IMU_Ki;

	gx = gx + IMU_Kp * ex + exInt;
	gy = gy + IMU_Kp * ey + eyInt;
	gz = gz + IMU_Kp * ez + ezInt;

	q0temp = q0;
	q1temp = q1;
	q2temp = q2;
	q3temp = q3;

	q0 = q0temp + (-q1temp * gx - q2temp * gy - q3temp * gz)*halfT;
	q1 = q1temp + (q0temp * gx + q2temp * gz - q3temp * gy)*halfT;
	q2 = q2temp + (q0temp * gy - q1temp * gz + q3temp * gx)*halfT;
	q3 = q3temp + (q0temp * gz + q1temp * gy - q2temp * gx)*halfT;

	norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;

	/* yaw */
	#if 1
	angle_z = angle_z + gz * halfT * 2 * 57.3f;

	x = angle_z / 360;

	temp = (angle_z - x * 360);

	if(temp < -180){
		mpu->angle_z = temp + 360;
	}else if(temp > 180){
		mpu->angle_z = temp - 360;
	}else{
		mpu->angle_z = temp;
	}
	#else
	mpu->angle_z = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
	#endif

	/* pitch */
	if(fabs(mpu->angle_x) < 90.0){
		mpu->angle_y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
	}else{
		mpu->angle_y  = (fabs(q0* q2 - q1 * q3)/(q0* q2 - q1 * q3)) *(180.0 - fabs(asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3));
	}
	/* roll */
	mpu->angle_x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f;
	mpu->q0 = q0;
	mpu->q1 = q1;
	mpu->q2 = q2;
	mpu->q3 = q3;
	return 0;
}

#ifdef __cplusplus
}
#endif		








